﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using IMIForUnity;
using UnityEngine;

namespace IMIForUnity
{
    public class BackToHome : ImiGesture
    {
        private float BackToHome_leftForearmTh_min = 35.0f;
        private float BackToHome_leftForearmTh_max = 60.0f;
        private float BackToHome_HoleTime = 3.0f;


        public BackToHome()
        {
            name = "BackToHome";
            type = Type.IMMEDIATE;
            startTime = currentTime = 0;
            state = ImiGesture.State.UNCHECKED;
        }

        public BackToHome(float BackToHome_leftForearmTh_min, float BackToHome_leftForearmTh_max, float Home_HoleTime)
        {
            name = "BackToHome";
            type = Type.IMMEDIATE;
            startTime = currentTime = 0;
            state = ImiGesture.State.UNCHECKED;
            this.BackToHome_HoleTime = Home_HoleTime;
            this.BackToHome_leftForearmTh_max = BackToHome_leftForearmTh_max;
            this.BackToHome_leftForearmTh_min = BackToHome_leftForearmTh_min;
        }

        public override State CheckGesture(ImiPlayerInfo playerInfo, double timestamp)
        {

            bool[] jointsTracked = playerInfo.GetPlayerJointsTracked();
            Vector3[] jointsPos = playerInfo.GetPlayerJointsPos();
            Vector3[] limbsAngular = playerInfo.GetPlayerLimbsAngular();

            switch (state)
            {
                case State.UNCHECKED:
                case State.CANCELED:
                    if (jointsTracked[leftHandIndex] && jointsTracked[leftElbowIndex] &&
                        jointsTracked[rightHandIndex] && jointsTracked[rightElbowIndex] && jointsTracked[rightShoulderIndex] &&
                        (jointsPos[leftHandIndex].y - jointsPos[leftElbowIndex].y) < 0 &&
                        (jointsPos[leftHandIndex].z - jointsPos[leftElbowIndex].z) > -0.17f &&
                        (jointsPos[leftHandIndex].z - jointsPos[leftElbowIndex].z) < 0.1f &&
                        limbsAngular[leftForearmIndex].x >= BackToHome_leftForearmTh_min && limbsAngular[leftForearmIndex].x <= BackToHome_leftForearmTh_max &&
                        (jointsPos[rightHandIndex].y - jointsPos[rightElbowIndex].y) < 0 &&
                        (jointsPos[rightElbowIndex].y - jointsPos[rightShoulderIndex].y) < 0 &&
                        Math.Abs(limbsAngular[rightForearmIndex].x) >= 60.0f && Math.Abs(limbsAngular[rightForearmIndex].x) <= 90.0f &&
                        Math.Abs(limbsAngular[rightPostbrachiumIndex].x) >= 60.0f && Math.Abs(limbsAngular[rightPostbrachiumIndex].x) <= 90.0f)
                    {
                        startTime = timestamp;
                        currentTime = timestamp;
                        state = State.CHECKING;
                    }
                    break;
                case State.CHECKING:
                    if (jointsTracked[leftHandIndex] && jointsTracked[leftElbowIndex] &&
                        jointsTracked[rightHandIndex] && jointsTracked[rightElbowIndex] && jointsTracked[rightShoulderIndex] &&
                        (jointsPos[leftHandIndex].y - jointsPos[leftElbowIndex].y) < 0 &&
                        (jointsPos[leftHandIndex].z - jointsPos[leftElbowIndex].z) > -0.17f &&
                        (jointsPos[leftHandIndex].z - jointsPos[leftElbowIndex].z) < 0.1f &&
                        limbsAngular[leftForearmIndex].x >= BackToHome_leftForearmTh_min && limbsAngular[leftForearmIndex].x <= BackToHome_leftForearmTh_max &&
                        (jointsPos[rightHandIndex].y - jointsPos[rightElbowIndex].y) < 0 &&
                        (jointsPos[rightElbowIndex].y - jointsPos[rightShoulderIndex].y) < 0 &&
                        Math.Abs(limbsAngular[rightForearmIndex].x) >= 60.0f && Math.Abs(limbsAngular[rightForearmIndex].x) <= 90.0f &&
                        Math.Abs(limbsAngular[rightPostbrachiumIndex].x) >= 60.0f && Math.Abs(limbsAngular[rightPostbrachiumIndex].x) <= 90.0f)
                    {

                        if (progress < 1.0f)
                        {
                            return CheckComplete(timestamp, BackToHome_HoleTime);
                        }
                    }
                    else
                    {
                        state = State.CANCELED;
                        progress = 0;
                    }
                    break;
            }
            return state;
        }
        //public override void GestureComplete(ImiPlayerInfo player)
        //{
        //    Debug.Log("Quiting!");
        //}
    }



    public class Calibration : ImiGesture
    {

        private float Calibration_horizontalForearmTh = 20.0f;//15.0f;
        private float Calibration_horizontalDif = 40.0f;//30.0f;
        private float Calibration_HoldTime = 1.2f;//1.5f;
        private float RaiseHand_Y = 0.2f;
        private float RaiseHand_HoldTime = 0.7f;//1.5;


        public Calibration()
        {
            name = "Calibration";
            type = Type.LASTING;
            startTime = currentTime = 0;
            state = State.UNCHECKED;
        }

        public Calibration(float Calibration_horizontalForearmTh, float Calibration_horizontalDif, float Calibration_HoldTime, float RaiseHand_Y, float RaiseHand_HoldTime)
        {
            name = "Calibration";
            type = Type.LASTING;
            startTime = currentTime = 0;
            state = State.UNCHECKED;
            this.Calibration_horizontalForearmTh = Calibration_horizontalForearmTh;
            this.Calibration_horizontalDif = Calibration_horizontalDif;
            this.Calibration_HoldTime = Calibration_HoldTime;
            this.RaiseHand_Y = RaiseHand_Y;
            this.RaiseHand_HoldTime = RaiseHand_HoldTime;
        }

        public override State CheckGesture(ImiPlayerInfo playerInfo, double timestamp)
        {
            bool[] jointsTracked = playerInfo.GetPlayerJointsTracked();
            Vector3[] jointsPos = playerInfo.GetPlayerJointsPos();
            Vector3[] limbsAngular = playerInfo.GetPlayerLimbsAngular();

            switch (state)
            {
                case State.UNCHECKED:
                case State.CANCELED:
                    if (jointsTracked[leftHandIndex] && jointsTracked[leftElbowIndex] && jointsTracked[leftShoulderIndex] &&
                        jointsTracked[rightHandIndex] && jointsTracked[rightElbowIndex] && jointsTracked[rightShoulderIndex] &&
                        (jointsPos[leftHandIndex].x - jointsPos[leftElbowIndex].x) < 0 &&
                        (jointsPos[rightHandIndex].x - jointsPos[rightElbowIndex].x) > 0 &&
                        Math.Abs(jointsPos[leftHandIndex].z - jointsPos[leftElbowIndex].z) < 0.3f &&
                        Math.Abs(jointsPos[rightHandIndex].z - jointsPos[rightElbowIndex].z) < 0.3f &&
                        Math.Abs(limbsAngular[leftForearmIndex].x) < Calibration_horizontalForearmTh && Math.Abs(limbsAngular[leftForearmIndex].x - limbsAngular[leftPostbrachiumIndex].x) < Calibration_horizontalDif &&
                        Math.Abs(limbsAngular[rightForearmIndex].x) < Calibration_horizontalForearmTh && Math.Abs(limbsAngular[rightForearmIndex].x - limbsAngular[rightPostbrachiumIndex].x) < Calibration_horizontalDif)
                    {
                        startTime = timestamp;
                        currentTime = timestamp;
                        state = State.CHECKING;
                    }
                    break;
                case State.CHECKING:
                    if (jointsTracked[leftHandIndex] && jointsTracked[leftElbowIndex] && jointsTracked[leftShoulderIndex] &&
                        jointsTracked[rightHandIndex] && jointsTracked[rightElbowIndex] && jointsTracked[rightShoulderIndex] &&
                        (jointsPos[leftHandIndex].x - jointsPos[leftElbowIndex].x) < 0 &&
                        (jointsPos[rightHandIndex].x - jointsPos[rightElbowIndex].x) > 0 &&
                        Math.Abs(jointsPos[leftHandIndex].z - jointsPos[leftElbowIndex].z) < 0.3f &&
                        Math.Abs(jointsPos[rightHandIndex].z - jointsPos[rightElbowIndex].z) < 0.3f &&
                        Math.Abs(limbsAngular[leftForearmIndex].x) < Calibration_horizontalForearmTh && Math.Abs(limbsAngular[leftForearmIndex].x - limbsAngular[leftPostbrachiumIndex].x) < Calibration_horizontalDif &&
                        Math.Abs(limbsAngular[rightForearmIndex].x) < Calibration_horizontalForearmTh && Math.Abs(limbsAngular[rightForearmIndex].x - limbsAngular[rightPostbrachiumIndex].x) < Calibration_horizontalDif)
                    {
                        if (progress < 1.0f)
                        {
                            return CheckComplete(timestamp, Calibration_HoldTime);
                        }
                    }
                    else
                    {
                        state = State.CANCELED;
                        progress = 0f;
                    }
                    break;
            }
            return state;
        }
    }





    public class RaiseRightHand : ImiGesture
    {
        private static float RaiseHand_HoldTime = 0.7f;//1.5f;
        private static float RaiseHand_Y = 0.2f;


        public RaiseRightHand()
        {
            name = "RaiseRightHand";
            type = Type.IMMEDIATE;
            startTime = currentTime = 0f;
            state = State.UNCHECKED;
            progress = 0;
        }
        public override State CheckGesture(ImiPlayerInfo playerInfo, double timestamp)
        {

            bool[] jointsTracked = playerInfo.GetPlayerJointsTracked();
            Vector3[] jointsPos = playerInfo.GetPlayerJointsPos();
            Vector3[] jointsSpeed = playerInfo.GetPlayerJointsSpeed();


            switch (state)
            {
                case State.UNCHECKED:
                case State.CANCELED:
                    if (jointsTracked[rightHandIndex] && jointsTracked[rightShoulderIndex] &&
                        (jointsPos[rightHandIndex].y - jointsPos[rightShoulderIndex].y) > RaiseHand_Y &&
                        jointsTracked[leftHandIndex] && jointsTracked[leftShoulderIndex] &&
                        (jointsPos[leftHandIndex].y - jointsPos[leftShoulderIndex].y) < 0)
                    {
                        startTime = timestamp;
                        currentTime = timestamp;
                        state = State.CHECKING;
                    }
                    break;
                case State.CHECKING:
                    if (jointsTracked[rightHandIndex] && jointsTracked[rightShoulderIndex] &&
                        (jointsPos[rightHandIndex].y - jointsPos[rightShoulderIndex].y) > RaiseHand_Y &&
                        jointsTracked[leftHandIndex] && jointsTracked[leftShoulderIndex] &&
                        (jointsPos[leftHandIndex].y - jointsPos[leftShoulderIndex].y) < 0 && Math.Abs(jointsSpeed[rightHandIndex].x) < 0.02f)
                    {
                        if (progress < 1.0f)
                        {
                            return CheckComplete(timestamp, RaiseHand_HoldTime);
                        }
                    }
                    else
                    {
                        state = State.CANCELED;
                        progress = 0;
                    }
                    break;
            }
            return state;
        }
    }

    public class RaiseLeftHand : ImiGesture
    {
        private static float RaiseHand_HoldTime = 0.7f;//1.5f;
        private static float RaiseHand_Y = 0.2f;


        public RaiseLeftHand()
        {
            name = "RaiseLeftHand";
            type = Type.LASTING;
            startTime = currentTime = 0;
            state = State.UNCHECKED;
        }

        public override State CheckGesture(ImiPlayerInfo playerInfo, double timestamp)
        {
            bool[] jointsTracked = playerInfo.GetPlayerJointsTracked();
            Vector3[] jointsPos = playerInfo.GetPlayerJointsPos();
            Vector3[] jointsSpeed = playerInfo.GetPlayerJointsSpeed();

            switch (state)
            {
                case State.UNCHECKED:
                case State.CANCELED:
                    
                    if (jointsTracked[leftHandIndex] && jointsTracked[leftShoulderIndex] &&
                        (jointsPos[leftHandIndex].y - jointsPos[leftShoulderIndex].y) > RaiseHand_Y &&
                        jointsTracked[rightHandIndex] && jointsTracked[rightShoulderIndex] &&
                        (jointsPos[rightHandIndex].y - jointsPos[rightShoulderIndex].y) < 0)
                    {
                        startTime = timestamp;
                        currentTime = timestamp;
                        state = State.CHECKING;
                    }
                    break;

                case State.CHECKING:
                    if (jointsTracked[leftHandIndex] && jointsTracked[leftShoulderIndex] &&
                        (jointsPos[leftHandIndex].y - jointsPos[leftShoulderIndex].y) > RaiseHand_Y &&
                        jointsTracked[rightHandIndex] && jointsTracked[rightShoulderIndex] &&
                        (jointsPos[rightHandIndex].y - jointsPos[rightShoulderIndex].y) < 0 && Math.Abs(jointsSpeed[leftHandIndex].x) < 0.015f)
                    {
                        if (progress < 1.0f)
                        {
                            return CheckComplete(timestamp, RaiseHand_HoldTime);
                        }
                    }
                    else
                    {
                        state = State.CANCELED;
                        progress = 0f;
                    }
                    break;
            }
            return state;
        }
    }






    public class Jump : ImiGesture
    {

        private int stage = 0;
        private float z = 0f;
        public Jump()
        {
            name = "Jump";
            type = Type.IMMEDIATE;
            startTime = currentTime = 0;
            state = State.UNCHECKED;
        }

        public override State CheckGesture(ImiPlayerInfo playerInfo, double timestamp)
        {
            bool[] jointsTracked = playerInfo.GetPlayerJointsTracked();
            Vector3[] jointsSpeed = playerInfo.GetPlayerJointsSpeed();
            Vector3[] jointsPos = playerInfo.GetPlayerJointsPos();

            switch (state)
            {
                case State.UNCHECKED:
                case State.CANCELED:
                    if (jointsTracked[ImiGesture.leftKneeIndex] && jointsTracked[ImiGesture.rightKneeIndex] &&
                        jointsTracked[ImiGesture.leftHipIndex] && jointsTracked[ImiGesture.rightHipIndex] &&
                        jointsSpeed[ImiGesture.hipCenterIndex].y > 0)
                    {
                        z = jointsPos[hipCenterIndex].z;
                        startTime = timestamp;
                        currentTime = timestamp;
                        state = State.CHECKING;
                        stage = 0;
                    }
                    break;
                case State.CHECKING:
                    if (stage == 0)
                    {
                        if ((timestamp - startTime) < 0.5f && jointsTracked[ImiGesture.hipCenterIndex] && jointsTracked[ImiGesture.leftShoulderIndex] && jointsTracked[rightShoulderIndex] &&
                            jointsTracked[ImiGesture.leftKneeIndex] && jointsTracked[ImiGesture.rightKneeIndex] && jointsSpeed[ImiGesture.hipCenterIndex].y > 0.02f && jointsSpeed[ImiGesture.leftShoulderIndex].y > 0.02f 
                            && jointsSpeed[ImiGesture.rightShoulderIndex].x > 0.02f && 
                            Math.Abs(jointsPos[ImiGesture.hipCenterIndex].z - z) < 0.1f)
                        {
                            currentTime = timestamp;
                            stage = 1;
                        }
                        else
                        {
                            state = State.CANCELED;
                            progress = 0;
                            stage = 0;
                        }
                    }
                    else
                    {
                        if ((timestamp - startTime) < 1.5f && jointsTracked[ImiGesture.hipCenterIndex] && jointsTracked[ImiGesture.leftShoulderIndex] && jointsTracked[ImiGesture.rightShoulderIndex] && Math.Abs(jointsPos[hipCenterIndex].z - z) < 0.1f)
                        {
                            if (jointsSpeed[ImiGesture.hipCenterIndex].y < 0 && jointsSpeed[leftShoulderIndex].y < 0 && jointsSpeed[rightHandIndex].y < 0)
                            {
                                return CheckComplete(timestamp, 0.1f);
                            }
                        }
                        else
                        {
                            state = State.CANCELED;
                            progress = 0;
                            stage = 0;
                        }
                    }
                    break;
            }
            return state;
        }
    }









    public class Run : ImiGesture
    {
        private double hzTimeStamp = 0;
        private bool leftKnee = false;
        private bool rightKnee = false;
        private float hz = 0.0f;
        private int progressState = 0;

        public Run()
        {
            name = "Run";
            type = Type.LASTING;
            startTime = currentTime = 0;
            state = State.UNCHECKED;
        }

        public override State CheckGesture(ImiPlayerInfo playerInfo, double timestamp)
        {

            bool[] jointsTracked = playerInfo.GetPlayerJointsTracked();
            Vector3[] jointsSpeed = playerInfo.GetPlayerJointsSpeed();

            switch (state)
            {
                case State.UNCHECKED:
                case State.CANCELED:
                    if (jointsTracked[ImiGesture.leftKneeIndex] && jointsTracked[ImiGesture.rightKneeIndex] &&
                        jointsSpeed[ImiGesture.rightKneeIndex].y > 0.01f && jointsSpeed[ImiGesture.leftKneeIndex].y < 0)
                    {
                        startTime = timestamp;
                        currentTime = timestamp;
                        state = State.CHECKING;
                        hzTimeStamp = timestamp;
                    }
                    break;
                case State.CHECKING:
                    if ((timestamp - currentTime) < 0.5f)
                    {
                        if (jointsTracked[ImiGesture.leftKneeIndex] && jointsTracked[ImiGesture.rightKneeIndex] &&
                            jointsSpeed[ImiGesture.leftKneeIndex].y > 0.01f && jointsSpeed[ImiGesture.rightKneeIndex].y < 0)
                        {
                            leftKnee = true;
                        }
                        //if (jointsTracked[ImiGestures.leftHandIndex] && jointsTracked[ImiGestures.leftElbowIndex] &&
                        //    jointsSpeed[ImiGestures.rightHandIndex].z > 0.1f && jointsSpeed[ImiGestures.leftHandIndex].z < 0)
                        //{
                        //    gestureData.rightHand = true;
                        //}
                        if (leftKnee)//&& gestureData.rightHand)
                        {
                            if (hzTimeStamp != 0)
                            {
                                //hz = 1 / (timestamp - hzTimeStamp);
                                hzTimeStamp = timestamp;
                            }

                            leftKnee = false;
                            //  gestureData.rightHand = false;
                            currentTime = timestamp;

                            progressState++;
                            if (progressState >= 2)
                            {
                                state = State.FINISHED;
                            }
                        }
                        if (jointsTracked[ImiGesture.leftKneeIndex] && jointsTracked[ImiGesture.rightKneeIndex] &&
                            jointsSpeed[ImiGesture.rightKneeIndex].y > 0.01f && jointsSpeed[ImiGesture.leftKneeIndex].y < 0)
                        {
                            rightKnee = true;

                        }
                        //if (jointsTracked[ImiGestures.leftHandIndex] && jointsTracked[ImiGestures.leftElbowIndex] &&
                        //    jointsSpeed[ImiGestures.leftHandIndex].z > 0.1f && jointsSpeed[ImiGestures.rightHandIndex].z < 0)
                        //{
                        //    gestureData.leftHand = true;
                        //}
                        if (rightKnee)//&& gestureData.leftHand)
                        {
                            rightKnee = false;
                            // gestureData.leftHand = false;
                            currentTime = timestamp;

                            progressState++;
                            if (progressState >= 2)
                            {
                                state = State.FINISHED;
                            }
                        }
                    }
                    else
                    {
                        rightKnee = false;
                        leftKnee = false;
                        progressState = 0;
                        state = State.CANCELED;
                        progress = 0;
                    }
                    break;

                case State.FINISHED:
                    state = State.CHECKING;
                    break;
            }
            return state;
        }
    }


    public class RightHandWave : ImiGesture
    {
        private bool stepOne = false;
        private bool stepTwo = false;

        private int joint = -1;
        private Vector3 jointPos;


        public RightHandWave()
        {
            name = "RightHandWave";
            type = Type.LASTING;
            startTime = currentTime = 0;
            state = State.UNCHECKED;
        }

        public override State CheckGesture(ImiPlayerInfo playerInfo, double timestamp)
        {
            //Debug.Log("Checking Right Hand Wave");
            bool[] jointsTracked = playerInfo.GetPlayerJointsTracked();
            Vector3[] jointsPos = playerInfo.GetPlayerJointsPos();
            Vector3[] jointsSpeed = playerInfo.GetPlayerJointsSpeed();

            switch (state)
            {
                case State.UNCHECKED:
                case State.CANCELED:
                    //Debug.Log("Unchecked  " + jointsTracked[rightHandIndex]+" " + jointsTracked[hipCenterIndex] +" "+ jointsPos[rightHandIndex].y +" "+ jointsPos[hipCenterIndex].y +" "+ jointsSpeed[rightHandIndex].x);
                    if (jointsTracked[rightHandIndex] && jointsTracked[hipCenterIndex] && 
                        jointsPos[rightHandIndex].y > jointsPos[hipCenterIndex].y && 
                        jointsSpeed[rightHandIndex].x > 0.02f)
                    {
                        startTime = timestamp;
                        stepOne = stepTwo = false;
                        currentTime = timestamp;
                        state = State.CHECKING;
                    }
                    break;

                case State.CHECKING:
                    //Debug.Log("Checking...");
                    if (!stepOne)
                    {
                        if ((timestamp - startTime) < 0.8f)
                        {
                            if (jointsTracked[rightHandIndex] && jointsTracked[hipCenterIndex] && 
                                jointsPos[rightHandIndex].y > jointsPos[hipCenterIndex].y && 
                                jointsSpeed[rightHandIndex].x < -0.02f)
                            {
                                stepOne = true;
                                currentTime = timestamp;
                                break;
                            }
                        }
                        else
                        {
                            state = State.CANCELED;
                            stepOne = false;
                            progress = 0;
                        }
                    }
                    if(stepOne)
                    {                     
                        if ((timestamp - startTime) < 1.8f)
                        {
                            if (jointsTracked[rightHandIndex] && jointsTracked[hipCenterIndex] && 
                                jointsPos[rightHandIndex].y > jointsPos[hipCenterIndex].y && 
                                jointsSpeed[rightHandIndex].x > 0.02f)
                            {
                                stepTwo = true;
                                currentTime = timestamp;
                            }

                            if (stepTwo == true)
                            {
                                return CheckComplete(timestamp, 0);
                            }
                        }
                        else
                        {
                            state = State.CANCELED;
                            stepOne = false;
                            stepTwo = false;
                            progress = 0;
                        }
                    }
                    break;
            }
            return state;
        }

        public override void GestureEvent(ImiGesture gesture, ImiPlayerInfo player, GestureEvent gevent)
        {
            int newMainPlayerID = player.GetUserId();
            ImiManager.GetInstance().SetControlPlayerId(newMainPlayerID);
            ImiPlayerInfo newMainPlayer = ImiManager.GetInstance().GetPlayerInfo(newMainPlayerID);

            Dictionary<System.Type, ImiGesture.OnGestureEvent> mainPlayerGestures = ImiGestureManager.GetInstance().GetMainPlayerGestures();
            foreach(System.Type type in mainPlayerGestures.Keys)
            {
                if (newMainPlayer.playerGestures.ContainsKey(type))
                {
                    ImiGesture g = (ImiGesture)Activator.CreateInstance(type);
                    g.GestureEventListeners += mainPlayerGestures[type];
                }
            }
        }
    }






    public class LeftHandWaveLeft : ImiGesture
    {

        private float y;


        public LeftHandWaveLeft()
        {
            name = "LeftHandWaveLeft";
            type = Type.IMMEDIATE;
            startTime = currentTime = 0;
            state = State.UNCHECKED;
        }

        public override State CheckGesture(ImiPlayerInfo playerInfo, double timestamp)
        {
            bool[] jointsTracked = playerInfo.GetPlayerJointsTracked();
            Vector3[] jointsPos = playerInfo.GetPlayerJointsPos();
            Vector3[] jointsSpeed = playerInfo.GetPlayerJointsSpeed();

            switch (state)
            {
                case State.UNCHECKED:
                case State.CANCELED:
                    if (jointsTracked[leftHandIndex] && jointsTracked[hipCenterIndex] && jointsPos[leftHandIndex].y > jointsPos[hipCenterIndex].y && jointsSpeed[leftHandIndex].x < -0.06f)
                    {
                        y = jointsPos[leftHandIndex].y;
                        startTime = timestamp;
                        currentTime = timestamp;
                        state = State.CHECKING;
                    }
                    else
                    {
                        state = State.CANCELED;
                        progress = 0;
                    }
                    break;
                case State.CHECKING:
                    float deltaTime = (float)(timestamp - startTime);
                    if (deltaTime >= 0.3f && deltaTime <= 1.0f && jointsTracked[leftHandIndex] && 
                        jointsTracked[hipCenterIndex] && jointsTracked[rightHandIndex] && 
                        Math.Abs(jointsPos[leftHandIndex].y - jointsPos[rightHandIndex].y) > 0.2)
                    {
                        if (Math.Abs(jointsPos[leftHandIndex].y - y) < 0.3 && Math.Abs(jointsSpeed[leftHandIndex].x) < 0.01f)
                        {
                            return CheckComplete(timestamp, 0);
                        }

                    }
                    else if (deltaTime > 1.0f || Math.Abs(jointsPos[leftHandIndex].y - jointsPos[rightHandIndex].y) < 0.2)
                    {
                        state = State.CANCELED;
                        progress = 0;
                    }
                    break;
            }
            return state;
        }
    }







    public class LeftHandWaveRight : ImiGesture
    {

        public LeftHandWaveRight()
        {
            name = "LeftHandWaveRight";
            type = Type.LASTING;
            startTime = currentTime = 0;
            state = State.UNCHECKED;
        }

        public override State CheckGesture(ImiPlayerInfo playerInfo, double timestamp)
        {
            bool[] jointsTracked = playerInfo.GetPlayerJointsTracked();
            Vector3[] jointsPos = playerInfo.GetPlayerJointsPos();
            Vector3[] jointsSpeed = playerInfo.GetPlayerJointsSpeed();

            switch (state)
            {
                case State.UNCHECKED:
                case State.CANCELED:
                    if (jointsTracked[leftHandIndex] && jointsTracked[hipCenterIndex] && 
                        jointsPos[leftHandIndex].y > jointsPos[hipCenterIndex].y && jointsSpeed[leftHandIndex].x > 0.06f)
                    {
                        startTime = timestamp;
                        currentTime = timestamp;
                        state = State.CHECKING;
                    }
                    else
                    {
                        state = State.CANCELED;
                        progress = 0;
                    }
                    break;
                case State.CHECKING:
                    float deltaTime = (float)(timestamp - currentTime);
                    if (deltaTime >= 0.4f && deltaTime <= 1.0f)
                    {
                        if (jointsTracked[leftHandIndex] && jointsTracked[leftShoulderIndex])
                        {
                            if (Math.Abs(jointsSpeed[leftHandIndex].x) < 0.01f && jointsPos[leftHandIndex].x - jointsPos[leftShoulderIndex].x > 0.1f)
                            {
                                return CheckComplete(timestamp, 0);
                            }
                        }
                    }
                    else if (deltaTime > 1.0f)
                    {
                        state = State.CANCELED;
                        progress = 0;
                    }
                    break;
            }
            return state;
        }
    }




    public class RightHandWaveLeft : ImiGesture
    {

        

        public RightHandWaveLeft()
        {
            name = "RightHandWaveLeft";
            type = Type.LASTING;
            startTime = currentTime;
            state = State.UNCHECKED;
        }

        public override State CheckGesture(ImiPlayerInfo playerInfo, double timestamp)
        {
            bool[] jointsTracked = playerInfo.GetPlayerJointsTracked();
            Vector3[] jointsPos = playerInfo.GetPlayerJointsPos();
            Vector3[] jointsSpeed = playerInfo.GetPlayerJointsSpeed();

            switch (state)
            {
                case State.UNCHECKED:
                case State.CANCELED:
                    if (jointsTracked[rightHandIndex] && jointsTracked[hipCenterIndex] && jointsPos[rightHandIndex].y > jointsPos[hipCenterIndex].y && jointsSpeed[rightHandIndex].x < -0.06f)
                    {
                        
                        startTime = timestamp;
                        currentTime = timestamp;
                        state = State.CHECKING;
                    }
                    else
                    {
                        state = State.CANCELED;
                        progress = 0;
                    }
                    break;
                case State.CHECKING:
                    float deltaTime = (float)(timestamp - currentTime);
                    if (deltaTime >= 0.4f && deltaTime <= 1.0f)
                    {
                        if (jointsTracked[rightHandIndex] && jointsTracked[rightShoulderIndex])
                        {
                            if (Math.Abs(jointsSpeed[rightHandIndex].x) < 0.01f && jointsPos[rightHandIndex].x - jointsPos[rightShoulderIndex].x < -0.1f)
                            {
                                return CheckComplete(timestamp, 0);
                            }
                        }

                    }
                    else if (deltaTime > 1.0f)
                    {
                        state = State.CANCELED;
                        progress = 0;
                    }
                    break;
            }
            return state;
        }
    }

    public class RightHandWaveRight : ImiGesture
    {

        private float y;
        private int stage = 0;
        private float rightHandx;

        public RightHandWaveRight()
        {
            name = "RightHandWaveRight";
            type = Type.LASTING;
            startTime = currentTime = 0;
            state = State.UNCHECKED;
        }

        public override State CheckGesture(ImiPlayerInfo playerInfo, double timestamp)
        {
            bool[] jointsTracked = playerInfo.GetPlayerJointsTracked();
            Vector3[] jointsPos = playerInfo.GetPlayerJointsPos();
            Vector3[] jointsSpeed = playerInfo.GetPlayerJointsSpeed();
            switch (state)
            {
                case State.UNCHECKED:
                case State.CANCELED:
                    if (jointsTracked[rightHandIndex] && jointsTracked[hipCenterIndex] && 
                        jointsPos[rightHandIndex].y > jointsPos[hipCenterIndex].y && 
                        jointsSpeed[rightHandIndex].x > 0.06f)
                    {
                        stage = 0;
                        startTime = timestamp;
                        currentTime = timestamp;
                        state = State.CHECKING;
                        rightHandx = jointsPos[rightHandIndex].x;
                    }
                    else
                    {
                        state = State.CANCELED;
                        progress = 0;
                    }
                    break;
                case State.CHECKING:
                    if (stage == 0)
                    {
                        float deltaTime = (float)(timestamp - startTime);
                        if (deltaTime >= 0.2f && deltaTime <= 0.5f && jointsTracked[rightHandIndex] &&
                            jointsTracked[leftHandIndex] && Math.Abs(jointsPos[leftHandIndex].y - jointsPos[rightHandIndex].y) > 0.2)
                        {
                            if (Math.Abs(jointsSpeed[rightHandIndex].y) < 0.02f)
                            {
                                stage = 1;
                                currentTime = timestamp;
                                y = jointsPos[rightHandIndex].y;
                            }

                        }
                        else if (deltaTime > 0.5f || Math.Abs(jointsPos[leftHandIndex].y - jointsPos[rightHandIndex].y) < 0.2)
                        {
                            state = State.CANCELED;
                            stage = 0;
                            progress = 0;
                        }
                    }
                    else
                    {
                        float deltaTime = (float)(timestamp - currentTime);
                        
                        if(deltaTime > 0.2f && jointsTracked[leftHandIndex] && 
                            jointsTracked[rightHandIndex] && 
                            Math.Abs(jointsPos[leftHandIndex].y - jointsPos[rightHandIndex].y) > 0.2 && 
                            Math.Abs(jointsPos[rightHandIndex].y - y) < 0.05 )
                        {
                            return CheckComplete(timestamp, 0);
                        }
                        else if(deltaTime > 0.2f)
                        {
                            state = State.CANCELED;
                            stage = 0;
                            progress = 0;
                        }
                    }
                    break;
            }
            return state;
        }
    }

    public class RightHandPush : ImiGesture
    {
        private float deltaTime = 0.8f;

        public RightHandPush()
        {
            name = "RightHandPush";
            type = Type.IMMEDIATE;
            startTime = currentTime = 0;
            state = State.UNCHECKED;
            progress = 0f;
        }

        public RightHandPush(float deltaTime)
        {
            name = "RightHandPush";
            type = Type.IMMEDIATE;
            startTime = currentTime = 0;
            state = State.UNCHECKED;
            progress = 0f;
            this.deltaTime = deltaTime;
        }

        public override State CheckGesture(ImiPlayerInfo playerInfo, double timestamp)
        {
            bool[] jointsTracked = playerInfo.GetPlayerJointsTracked();
            Vector3[] jointsPos = playerInfo.GetPlayerJointsPos();
            Vector3[] jointsSpeed = playerInfo.GetPlayerJointsSpeed();

            switch (state)
            {
                case State.UNCHECKED:
                case State.CANCELED:
                    if (jointsTracked[rightHandIndex] && jointsPos[rightHandIndex].y > jointsPos[hipCenterIndex].y && jointsSpeed[rightHandIndex].z < -0.04f && Math.Abs(jointsSpeed[rightHandIndex].x) < 0.01f)
                    {
                        startTime = currentTime = timestamp;
                        state = State.CHECKING;
                    }
                    else
                    {
                        state = State.CANCELED;
                        progress = 0;
                    }
                    break;
                case State.CHECKING:
                    float dt = (float)(timestamp - startTime);
                    if (dt < deltaTime && jointsTracked[rightHandIndex] && Math.Abs(jointsSpeed[rightHandIndex].z) <= 0.04f)
                    {
                        state = State.FINISHED;
                        currentTime = timestamp;
                        progress = 1f;
                    }
                    else if (dt > deltaTime)
                    {
                        state = State.CANCELED;
                        progress = 0;
                    }
                    break;
            }

            return state;
        }
    }

    public class LeftHandPush : ImiGesture
    {
        private float deltaTime = 0.8f;
        public LeftHandPush()
        {
            name = "LeftHandPush";
            type = Type.IMMEDIATE;
            startTime = currentTime;
            state = State.UNCHECKED;
        }

        public LeftHandPush(float deltaTime)
        {
            name = "LeftHandPush";
            type = Type.IMMEDIATE;
            startTime = currentTime;
            state = State.UNCHECKED;
            this.deltaTime = deltaTime;
        }

        public override State CheckGesture(ImiPlayerInfo playerInfo, double timestamp)
        {
            bool[] jointsTracked = playerInfo.GetPlayerJointsTracked();
            Vector3[] jointsPos = playerInfo.GetPlayerJointsPos();
            Vector3[] jointsSpeed = playerInfo.GetPlayerJointsSpeed();

            switch (state)
            {
                case State.UNCHECKED:
                case State.CANCELED:
                    if (jointsTracked[leftHandIndex] && jointsTracked[leftHandIndex] && 
                        jointsPos[leftHandIndex].y > jointsPos[hipCenterIndex].y && 
                        jointsSpeed[leftHandIndex].z < -0.06f && Math.Abs(jointsSpeed[leftHandIndex].x) < 0.01f)
                    {
                        startTime = currentTime = timestamp;
                        state = State.CHECKING;
                    }
                    else
                    {
                        state = State.CANCELED;
                        progress = 0;
                    }
                    break;
                case State.CHECKING:
                    float dt = (float)(timestamp - startTime);
                    if (dt < deltaTime && jointsTracked[leftHandIndex] && Math.Abs(jointsSpeed[leftHandIndex].z) <= 0.04f)
                    {
                        state = State.FINISHED;
                        currentTime = timestamp;
                        progress = 1f;
                    }
                    else if (dt > deltaTime)
                    {
                        state = State.CANCELED;
                        progress = 0;
                    }
                    break;
            }
            return state;
        }
    }

}
